#include "robot_orientation.h"
#include <math.h>
using namespace cv;

RobotOrientation getRobotOrientation(Point red, Point green) {
	RobotOrientation robot;

	if (red.x == green.x){
		if (red.y > green.y){
			robot.angle = 0;
		} else {
			robot.angle = 180;
		}
	} else {
		double dy = green.y - red.y;
		double dx = green.x - red.x;
		
		double phi = atan2(dy, dx) / M_PI * 180;
		robot.angle = phi - 90;
		
		if(robot.angle<0)
			robot.angle += 360;
	}

	robot.point.x = (red.x + green.x) / 2;
	robot.point.y = (red.y + green.y) / 2;
	
	return robot;
}
